#include <algorithm>
#include <iostream>
#include <fstream>
#include <iterator>

#include "DataManager.h"
#include "../../DataStructure/Frame.h"
#include "../../DataStructure/LandMarkPLX.h"
#include "../../util/mathfunc.h"

namespace Estimator
{
void Managing::AddFrame(Frame* frame)
{
    // for each observation , check the main anchor and associate anchor.
    // for(const auto& landmarkobs : frame->mvLandMarkObservations_)
    for(size_t i = 0 ; i < frame->mvLandMarkObservations_.size(); ++i)
    {
        const auto& landmarkobs = frame->mvLandMarkObservations_[i];

        // search for the landmark according to their id
        auto search_pos = std::find_if(mlLandMarkContainer_.begin(), mlLandMarkContainer_.end(), 
                                    [&landmarkobs](const LandMarkPLX& plandmark){ return plandmark.GetId() == landmarkobs.first; });

        if (search_pos != mlLandMarkContainer_.end())
        {
            // Add frame and corresponding index into container.. 
            search_pos->AddObservation(frame, i);

            //LandMarkPLX* tempFeature = *search_pos;
            if (search_pos->GetStatus() == LandMarkPLX::READY)
                continue;
            else // if not ready or need more frame:
            {
                double parallax_threshold = 0.5403;//0.8776; // cos(0.5);
                // Get Direction Vector(not normalized) in current frame.
                Eigen::Vector3d dirLandMark = mpCalibration_->Kinv * Eigen::Vector3d(landmarkobs.second[0], landmarkobs.second[1], 1);
                dirLandMark.normalize();
                // dirLandMark will be normalized in GetCosParallax anyway.
                const auto *main_anchor = search_pos->GetMainFrame();
                double inner_product = Estimator::GetCosParallax(frame->mRotationMatrix, main_anchor->mRotationMatrix, dirLandMark, search_pos->GetDirectionMain(), mpCalibration_->Rbc);
                // std::cout << inner_value << std::endl;
                if (inner_product < search_pos->GetCosParallax())
                {
                    if (inner_product < parallax_threshold)
                        search_pos->SetAssociateFrame(frame, inner_product, LandMarkPLX::READY); // set to be ready and won't change associate anchor anymore
                    else
                        search_pos->SetAssociateFrame(frame, inner_product, LandMarkPLX::NEED_MORE_FRAME); // still need more frames for bigger parallax.
                }
                continue;
            }
        }
        else // if not inside feature ready container, that means inserting a new point..
        {
            Eigen::Vector3d dirLandMark = mpCalibration_->Kinv * Eigen::Vector3d(landmarkobs.second[0], landmarkobs.second[1], 1);

            dirLandMark.normalize();
            // std::cout << dirLandMark << std::endl;
            mlLandMarkContainer_.emplace_back(landmarkobs.first, frame, dirLandMark);

            mlLandMarkContainer_.back().AddObservation(frame, i);
            // std::cout <<"Landmark id: "<< mlLandMarkContainer_.back().GetId() <<"Main Frame id: "  << mlLandMarkContainer_.back().GetMainFrame()->GetFrameId() << std::endl;

            continue;
        }
    }

    mvpFrameContainer_.push_back(frame);

}

void Managing::RemoveLandMarkWithSingleAnchor()
{
    mlLandMarkContainer_.erase(std::remove_if(mlLandMarkContainer_.begin(), mlLandMarkContainer_.end(),
                                              [](LandMarkPLX &landmark) { return (landmark.GetAssoFrame() == nullptr); }),
                               mlLandMarkContainer_.end());
}

void Managing::AddNoiseToRotations()
{
    for(auto ite = mvpFrameContainer_.begin(); ite != mvpFrameContainer_.end(); ++ite )
    {
        (*ite)->mRotationMatrix = ((*ite)->mRotationMatrix * RandomRotation(0.05)).eval();
    }
}

void Managing::SimplyOutPutParallax(std::string filename)
{
    // create a file and write the value inside the file.
    std::fstream file( filename + "GroundTruthLandMarkValue.txt", std::fstream::out | std::fstream::trunc);

    std::copy(mlLandMarkContainer_.cbegin(), mlLandMarkContainer_.cend(),
            std::ostream_iterator<LandMarkPLX>(file, "\n"));
    file.close();
}

void Managing::SetInitializationParameters()
{
    for (auto ite = mlLandMarkContainer_.begin(); ite != mlLandMarkContainer_.end(); ite++)
    {
        ite->SetInitializationParameter(*mpCalibration_);
    }
}

std::ostream& operator << (std::ostream& os, const Managing& manager)
{
    // os << timer.timer_name_ << " takes " << std::chrono::duration_cast<std::chrono::microseconds>(timer.end_ - timer.start_).count() << "us. ";

    for(auto landmarkplx : manager.mlLandMarkContainer_)
    {
      // if(landmarkplx.GetAssoFrame())
        os << landmarkplx << std::endl;
    }

    return os;
}

}